/*====================================*/
/*  Include                           */
/*====================================*/
#include <avr/io.h>
#include "speed.h"

/*====================================*/
/*  Function name: speed_init         */
/*  Parameter: null                   */
/*  Description: configure hardware   */
/*               for PWM (speed)      */
/*               function             */
/*====================================*/
void speed_init(){
	DIR_DDR  |=  _BV(DIR_LEFT)|_BV(DIR_RIGHT);

	PWM_DDR  |=  _BV(PWM_LEFT)|_BV(PWM_RIGHT);

	// Timer/Counter 0 initialization
	// Clock source: System Clock
	// Clock value: 2000.000 kHz
	// Mode: Phase correct PWM top=FFh
	// OC0 output: Non-Inverted PWM
	ASSR=0x00;
	TCCR0=0x62;
	TCNT0=0x00;
	OCR0=0x00;

	// Timer/Counter 2 initialization
	// Clock source: System Clock
	// Clock value: 2000.000 kHz
	// Mode: Phase correct PWM top=FFh
	// OC2 output: Non-Inverted PWM
	TCCR2=0x62;
	TCNT2=0x00;
	OCR2=0x00;

}

/*====================================*/
/*  Function name: set_left_dir       */
/*  Parameter: _dir                   */
/*  Description: setting direction for*/
/*             left motor             */
/*====================================*/
void set_left_dir(unsigned char _dir){
	if(_dir == FORWARD)
		DIR_POUT &= ~_BV(DIR_LEFT);
	else
		DIR_POUT |= _BV(DIR_LEFT);
}

/*====================================*/
/*  Function name: set_right_dir      */
/*  Parameter: _dir                   */
/*  Description: setting direction for*/
/*             right motor            */
/*====================================*/
void set_right_dir(unsigned char _dir){
	if(_dir == FORWARD)
		DIR_POUT &= ~_BV(DIR_RIGHT);
	else
		DIR_POUT |= _BV(DIR_RIGHT);
}

/*====================================*/
/*  Function name: speed              */
/*  Parameter:_left_speed,_right_speed*/
/*  Description: setting speed for    */
/*             left and right motor   */
/*====================================*/
void speed(int _left_speed, int _right_speed){

	if(_left_speed >= 0){
		set_left_dir(FORWARD);
	}else{
		_left_speed = -_left_speed;
		set_left_dir(BACKWARD);
	}
	if(_right_speed >= 0){
		set_right_dir(FORWARD);
	}else{
		_right_speed = -_right_speed;
		set_right_dir(BACKWARD);
	}

	if(_left_speed > MAX_SPEED)  _left_speed = MAX_SPEED;
	if(_left_speed < MIN_SPEED)  _left_speed = 0;
	
	if(_right_speed > MAX_SPEED)  _right_speed = MAX_SPEED;
	if(_right_speed < MIN_SPEED)  _right_speed = 0;

	LEFT_SPEED  = _left_speed;
	RIGHT_SPEED = _right_speed;
}


/****************END FILE**************/

